#include "Tasks.h"

/*****************************************************************************     
 * Variable Declarations                                                     *   
 *****************************************************************************/

/* Tasks Prototypes */
TASK Task_a;
TASK Task_b;
TASK Task_c;

/* Pointers */
TASK  *fptr;  
TASK  *lptr;

unsigned char case_probe=0;

/* Tasks Buffers */
TASK  fifoTask [fifoSize];   
unsigned char size;

/* Context Variables */
unsigned char  var     @ 0x380   = 1;
unsigned int   sp      @ spAddH  = 2; 
unsigned char  aReg    @ aAdd    = 3;
unsigned char  hReg    @ hAdd    = 4;
unsigned char  xReg    @ xAdd    = 5;
unsigned char  ccrReg  @ ccrAdd  = 6; 
unsigned int   spStak  @ spSAddH = 0x1234;  
unsigned int   spInte  @ spIntH  = 7;
unsigned char  aInte   @ aInt    = 8;
unsigned char  hInte   @ hInt    = 9;
unsigned char  xInte   @ xInt    = 10;
unsigned char  ccrInte @ ccrInt  = 11; 
unsigned int  brinco   @ varCheck = 0x22D0;

/* Stack Pointer Saved */ 
unsigned int  goBackSP @ sPointH = 0x0000;
unsigned char begining=1;
unsigned char alarmIndex=0;
unsigned char taskIndex=0;
unsigned int  tickCounter=0;
unsigned int  baseTickCounter=0;

unsigned char index;

/* Pointers */
TASK  *fptr;  
TASK  *lptr;

/* Tasks Buffers */
TASK  fifoTask [fifoSize];   
unsigned char size;

/*****************************************************************************     
 * Constructors                                                              *   
 *****************************************************************************/

void newConstructorDefault(TASK* thisTask, unsigned char u8thisFunctionID)
{
  thisTask->u8activation=ACTIVATE;
  thisTask->u8autostart=FALSE;
  thisTask->u8contex=NOCONTEXTED;
  thisTask->u8priority=0;
  thisTask->u8schedule=FULL;
  thisTask->u8state=SUSPENDED;
  thisTask->u8functionID=u8thisFunctionID;
}

void newConstructorID_P_AUSTR(TASK* thisTask, unsigned char u8thisFunctionID,
unsigned char u8thisPriority, unsigned char u8thisAutostart)
{
  thisTask->u8activation=ACTIVATE;
  thisTask->u8autostart=u8thisAutostart;
  thisTask->u8contex=NOCONTEXTED;
  thisTask->u8priority=u8thisPriority;
  thisTask->u8schedule=FULL;
  thisTask->u8state=SUSPENDED;
  thisTask->u8functionID=u8thisFunctionID;
}

void newConstructor(TASK* thisTask, unsigned char u8thisPriority, 
unsigned char u8thisSchedule, unsigned char u8thisAutostart,
unsigned char u8thisActivation, unsigned char u8thisState, 
unsigned char u8thisContex, unsigned char u8thisFunctionID)
{
  thisTask->u8activation=u8thisActivation;
  thisTask->u8autostart=u8thisAutostart;
  thisTask->u8contex=u8thisContex;
  thisTask->u8priority=u8thisPriority;
  thisTask->u8schedule=u8thisSchedule;
  thisTask->u8state=u8thisState;
  thisTask->u8functionID=u8thisFunctionID;
}

unsigned char taskGetFunctionID(TASK* thisTask)
{
  return thisTask->u8functionID; 
}

unsigned char taskGetPriority(TASK* thisTask)
{
  return thisTask->u8priority; 
}

unsigned char  taskGetSchedule(TASK* thisTask)
{
  return thisTask->u8schedule;
}

unsigned char  taskGetAutostart(TASK* thisTask)
{
  return thisTask->u8autostart;
}

unsigned char  taskGetActivation(TASK* thisTask)
{
  return thisTask->u8activation;
}

unsigned char  taskGetState(TASK* thisTask)
{
  return thisTask->u8state;
} 
 
unsigned char  taskGetContex(TASK* thisTask)
{
  return thisTask->u8contex;
}  

void  taskSetFunctionID(TASK* thisTask, unsigned char u8thisFunctionID)
{
  thisTask->u8functionID=u8thisFunctionID;
}  
 
void  taskSetPriority(TASK* thisTask, unsigned char u8thisPriority)
{
  thisTask->u8priority=u8thisPriority;
}
 
void  taskSetSchedule(TASK* thisTask, unsigned char u8thisSchedule)
{
  thisTask->u8schedule=u8thisSchedule;
}

void  taskSetAutostart(TASK* thisTask, unsigned char u8thisAutostart)
{
  thisTask->u8autostart=u8thisAutostart;
}

void  taskSetActivation(TASK* thisTask, unsigned char u8thisActivation)
{
  thisTask->u8activation=u8thisActivation;
}

void  taskSetState(TASK* thisTask, unsigned char u8thisState)
{
  thisTask->u8state=u8thisState;
} 
 
void  taskSetContex(TASK* thisTask, unsigned char u8thisContex)
{
  thisTask->u8contex=u8thisContex;
} 

void  copyTask(TASK* task2Copy, TASK* taskCopied)
{
 taskCopied->u8contex=task2Copy->u8contex;
 taskCopied->u8priority=task2Copy->u8priority;
 taskCopied->u8schedule=task2Copy->u8schedule;
 taskCopied->u8autostart=task2Copy->u8autostart;
 taskCopied->u8activation=task2Copy->u8activation;
 taskCopied->u8state=task2Copy->u8state;
 taskCopied->u8functionID=task2Copy->u8functionID;
 taskCopied->funcVoid=task2Copy->funcVoid;
}

/*****************************************************************************
 * Task Functions                                                            *
 *****************************************************************************/
void TASK_B(void)
{  
#ifdef PROBE1 
  /* Task_a sent to wait until task_b activate the mailbox */
  CancelAlarm(&ALARM_A);
  activaMailbox(MAILBOX1,1);
#endif  
#ifdef PROBE2
  /* Task_b take  the mailbox that task_a activate */
  tomaMailbox(MAILBOX1);
#endif  
#ifdef PROBE3
  /* Task_b activate the mailbox more tahn 1 time, if the mailbox,
   * is active task b is sent to wait */
  SetRelAlarm(&ALARM_A,3,2);
  activaMailbox(MAILBOX1,1);
#endif  
#ifdef PROBE4
  /* Task_b activate the mailbox more tahn 1 time, if the mailbox, is active 
   * task b is sent to wait, when tak c get the mailbox task b executed 
   * again */
  SetRelAlarm(&ALARM_A,3,2);
  if(case_probe>=2)
  {
    CancelAlarm(&ALARM_A);
    chainTask(&Task_c);
  }
  else
  {
    activaMailbox(MAILBOX1,1);
    case_probe++;
  }
#endif
  terminateTask(&Task_b);   
} 

void TASK_C(void)
{ 
#ifdef PROBE4
 /* Task_b activate the mailbox more tahn 1 time, if the mailbox, is active 
  * task b is sent to wait, when tak c get the mailbox task b executed 
  * again */
 tomaMailbox(MAILBOX1);
#endif 
 terminateTask(&Task_c);   
}  

void TASK_A(void)
{  
#ifdef PROBE1 
  /* Task_a sent to wait until task_b activate the mailbox */
  SetAbsAlarm (&ALARM_A,3,0);
  tomaMailbox(MAILBOX1);
#endif 
#ifdef PROBE2
  /* Task_b take  the mailbox that task_a activate */
  activaMailbox(MAILBOX1,1);
  chainTask(&Task_b);
#endif
#ifdef PROBE3
  /* Task_b activate the mailbox more tahn 1 time, if the mailbox,
   * is active task b is sent to wait */
  SetAbsAlarm (&ALARM_A,3,0);
#endif  
#ifdef PROBE4
  /* Task_b activate the mailbox more tahn 1 time, if the mailbox,is active 
   * task b is sent to wait, when tak c get the mailbox task b executed 
   * again */
  SetAbsAlarm (&ALARM_A,3,0);
#endif 
  terminateTask(&Task_a);  
}

/*****************************************************************************
 * Buffer de tareas que esperan ser sacadas                                  *
 *****************************************************************************/
TASK ejectTask(void)
{
  TASK nTask;  
  if(size)
  {    
    nTask=*fptr;
    size--;    
    if (fptr < &fifoTask[0] + 9){
      fptr++; 
    }else{
      fptr=&fifoTask[0];
    }    
  }
  else
  {
    /* Esta vacio */
  }
  return nTask;
}

TASK* runTask(void)
{
  return fptr;
}

void insertTask(TASK* thisTask)
{
  if(size < 10)
  {
    *lptr = *thisTask;
    size++;  
    if (lptr < &fifoTask[9])
    {
      lptr++; 
    }
    else
    {
      lptr=&fifoTask[0];
    }  
  }
  else
  {
  /* Esta lleno */
  } 
}

void taskPrioritize(void)
{  
  TASK temporal;
  TASK  *lptrLess2=lptr-2;
  TASK  *lptrLess1=lptr-1;
  unsigned char counter=0;    
  if(size > 1)
  {  
    do
    {
      if(lptrLess1 < &fifoTask[0])
      {
        lptrLess1=&fifoTask[9];
      }      
      if(lptrLess2 < &fifoTask[0])
      {
        lptrLess2=&fifoTask[9];
      }            
      if( lptrLess2->u8priority < lptrLess1->u8priority )
      {
        temporal=*(lptrLess2);
        *(lptrLess2)=*(lptrLess1);
        *(lptrLess1)=temporal;  
        lptrLess2--;
        lptrLess1--;
      }
      else
      {
        break; 
      }      
      counter++;   
    }while(counter < size-1);    
  }
}

void initOS(void)
{
  unsigned char posAlarmSchedule=9;
  fptr=&fifoTask[0];
  lptr=&fifoTask[0];
  size=0;
  /* Task initialisation */
  newConstructorID_P_AUSTR(&Task_a,task_a,0,TRUE);
  newConstructorID_P_AUSTR(&Task_b,task_b,1,FALSE);
  newConstructorID_P_AUSTR(&Task_c,task_c,2,FALSE);
  Task_a.funcVoid=TASK_A;
  Task_b.funcVoid=TASK_B;
  Task_c.funcVoid=TASK_C; 
  if(Task_a.u8autostart)
  {
    Task_a.u8state=READY;
    Task_a.u8activation=ACTIVATE;
    insertTask(&Task_a);
  }
  else
  {
    if(Task_b.u8autostart)
    {
      Task_b.u8state=READY;
      Task_b.u8activation=ACTIVATE;
      insertTask(&Task_b);
    }
    else
    {    
      if(Task_c.u8autostart)
      {
        Task_c.u8state=READY;
        Task_c.u8activation=ACTIVATE;
        insertTask(&Task_c);
      }
      else
      {
      
      }
    }
  }
  /* Init alarm object */                    
  do
  {                                   
  alarmSchedule[posAlarmSchedule]=NULL; 
  }while(posAlarmSchedule--);           
  SYSTEMTIMER.MINCYCLE=1;               
  SYSTEMTIMER.MAXALLOWEDVALUE=65535;    
  SYSTEMTIMER.TICKSPERBASE=2;                                                   
  ALARM_A.COUNT_OBJECT=SYSTEMTIMER;     
  ALARM_A.ACTION= ACTIVATETASK;         
  ALARM_A.A_AUTOSTART=FALSE;            
  ALARM_A.TASK_ALARM=&Task_b;           
  configTimerAlarm();                     
  initializeWaitedTasks();                                                                    
  initializeMailboxArray();                                                    
  configTimer();  
  portInit(); 
}
 
/*****************************************************************************
 * System Functions                                                          *
 *****************************************************************************/
void returnFromInterruption(void)
{

}

void activateTask(TASK* thisTask)
{
  /* Guardo el contexto de la tarea corriendo */
  asm
  {
    TSX
    STHX goBackSP
    /* Save the ccr, acumulator, h and x registers */
    /* H-X register */ 
    STHX spSAddH 
    /* Get H */ 
    LDHX #hAdd                 
    MOV  spSAddH,X+
    /* Get X */   
    LDHX #xAdd                             
    MOV  spSAddL,X+
    /* Get Accumulator */ 
    STA  spSAddH
    LDHX #aAdd
    MOV  spSAddH,X+
    /* Get CCR */
    TPA
    STA  spSAddH
    LDHX #ccrAdd
    MOV  spSAddH,X+ 
    /* Save the stack pointer */
    LDHX 3,SP       /* Load in HX register the sp value from the stak */ 
    STHX spSAddH    /* Save the HX register value in  spStak */
    LDHX #spAddH    /* Load to HX the direction of the first byte where */
                    /* I will save the firt bytes PC value */
    MOV  spSAddH,X+ /* Save the first byte PC in the direction 0x181 */
    LDHX #spAddL    /* Load to HX the direction of the second byte where */
                    /* I will save the firt bytes PC value */
    MOV  spSAddL,X+ /* Save the second byte PC in the direction 0x182 */
  }
  goBackSP=goBackSP+4;
  thisTask->u8activation=ACTIVATE; 
  thisTask->u8state=READY;
  fptr->u8contex=CONTEXTED;
  /* Salvar el contexto en la estructura */
  fptr->u16stackPointer=sp;
  fptr->u8aRegister=aReg;
  fptr->u8hRegister=hReg;
  fptr->u8xRegister=xReg;
  fptr->u8ccrRegister=ccrReg;
  fptr->u16goBackSP=goBackSP;
  insertTask(thisTask);
  taskPrioritize(); 
  asm
  {
    PULX
    PULX
    PULX
    PULX  
    LDHX #loopFor 
    JMP ,X
  } 
}

/*****************************************************************************
 * Funcion de activacion de desde interrupcion                               *
 *****************************************************************************/
void interruptActivateTask(TASK* thisTask)
{
  thisTask->u8activation=ACTIVATE;
  thisTask->u8state=READY;
  insertTask(thisTask);        
}

/*****************************************************************************
 * Funcion de Activacion de Tareas                                           *
 *****************************************************************************/
void terminateTask(TASK* thisTask)
{
  thisTask->u8activation=ACTIVATE;
  thisTask->u8state=READY;
  (void)ejectTask();
}

void chainTask(TASK* thisTask)
{
  insertTask(thisTask);  
}

void goBack(void)
{
  /* Recupero el contexto */
  asm
  {
    PULH
    PULX
    LDHX sp
    PSHX
    PSHH
    /* Put SR */    
    LDA ccrReg 
    TAP
    /* Put A */    
    LDA aReg
    /* Put H */      
    LDHX hReg
    /* Put X */      
    LDX xReg         
  }  
}